#ifndef UR_KIN_H
#define UR_KIN_H

class UrKinematics
{
  public:
    UrKinematics(double d1In, double a2In, double a3In, double d4In, double d5In, double d6In);


    // @param q       The 6 joint values
    // @param T       The 4x4 end effector pose in row-major ordering
    int forward(const double* q, double* T);
    int forward_bias(const double* q, double* T);
    

    
    // @param T       The 4x4 end effector pose in row-major ordering
    // @param q_sols  An 8x6 array of doubles returned, all angles should be in [0,2*PI)
    // @param q6_des  An optional parameter which designates what the q6 value should take
    //                in case of an infinite solution on that joint.
    // @return        Number of solutions found (maximum of 8)
    int inverse(const double* T, double* q_sols, double q6_des=0.0);
    int inverse_bias(const double* T, double* q_sols, double q6_des=0.0);
    bool getSinguratyResult(const double* T, double j3In, double j5In);

private:
    double ZERO_THRESH;
    double PI ;
    double d1 ;
    double a2 ;
    double a3 ;
    double d4 ;
    double d5 ;
    double d6 ;
    double singuratyRadius;
    double maxBallRadius;
};

#endif //UR_KIN_H
